#include "coordMath.h"

ECEFvector NEDtoECEF( NEDvector coord, LLHvector home) {

    double cosLat = cos(home.lat);
    double sinLat = sin(home.lat);

    double cosLon = cos(home.lon);
    double sinLon = sin(home.lon);

    double conversionMatrix [3][3] =
    {{-sinLat * cosLon, -sinLat * sinLon, cosLat},
     {-sinLon,          cosLon,           0.0},
     {-cosLat * cosLon, -cosLat * sinLon, -sinLat}};

    ECEFvector temp;
    temp.x = conversionMatrix[0][0] * coord.north + conversionMatrix[0][1] * coord.east + conversionMatrix[0][2] * coord.down;
    temp.y = conversionMatrix[1][0] * coord.north + conversionMatrix[1][1] * coord.east + conversionMatrix[1][2] * coord.down;
    temp.z = conversionMatrix[2][0] * coord.north + conversionMatrix[2][1] * coord.east + conversionMatrix[2][2] * coord.down;

    return temp;
} /* end NEDtoECEF */
